//------------------------------------------------------------------------------
//  <copyright file="FeedbackMessageManaged.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once
#include <ClientMessages.h>

using namespace System;

namespace Microsoft 
{
    namespace Robotics
    {
        namespace Hardware
        {
            namespace KukaLBR
            {
                /// <summary>
                /// Managed version of feedback registers.
                /// Register values are copied from the corresponding unmanaged versions
                /// </summary>
                public ref class FeedbackMessageManaged
                {
                public:
                    /// <summary>
                    /// The controller strategy as defined by KRL (Kuka).
                    /// Determines type of control.  Not all command registers
                    /// have an effect in all control modes.  
                    /// </summary>
                    double ControllerStrategyCommandRegister;

                    /// <summary> Commanded joint poisitions.  </summary>
                    array<double>^ JointPositionCommandRegister;

                    /// <summary> Commanded Cartesian poisitions.  </summary>
                    array<double>^ CartesianPositionCommandRegister;

                    /// <summary> Commanded joint torques.  </summary>
                    array<double>^  JointTorqueCommandRegister;

                    /// <summary> Commanded force/torque at the tool control point.  </summary>
                    array<double>^  TCPForceTorqueCommandRegister;

                    /// <summary> Commanded joint stiffness.  </summary>
                    array<double>^  JointStiffnessCommandRegister;

                    /// <summary> Commanded joint damping.  </summary>
                    array<double>^  JointDampingCommandRegister;

                    /// <summary> Commanded Cartesian stiffness.  </summary>
                    array<double>^  CartesianStiffnessCommandRegister;

                    /// <summary> Commanded Cartesian damping.  </summary>
                    array<double>^  CartesianDampingCommandRegister;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar1Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar2Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar3Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar4Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar5Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar6Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar7Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar8Register;

                    /// <summary> General purpose 6-dim vector.  </summary>
                    array<double>^  CartVec1Register;

                    /// <summary> General purpose 6-dim vector.  </summary>
                    array<double>^  CartVec2Register;

                    /// <summary> General purpose 6-dim vector.  </summary>
                    array<double>^  CartVec3Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec1Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec2Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec3Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame1Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame2Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame3Register;

					/// <summary> Number of the state that the current controller is in. </summary>
					double CurrentControllerStateNumber;

                    /// <summary> Timestamp of the FRI request. </summary>
                    double TimestampInMsRegister;

                    /// <summary> State of the FRI connection (i.e. Monitor, Command, etc...)  </summary>
                    double FriStateRegister;

                    /// <summary> Quality of the UDP connection. </summary>
                    double UdpQualityRegister;

                    /// <summary> Configured rate of FRI requests. </summary>
                    double CycleTimeInMsRegister;

                    /// <summary> Speed limit (in %) </summary>
                    double SafetyLimitInPercentRegister;

                    /// <summary> Indicates drive power status. </summary>
                    double DrivePowerBitFieldRegister;

                    /// <summary> Indicates current controller strategy </summary>
                    double ControllerStrategyRegister;

                    /// <summary> Joint temperature. </summary>
                    array<double>^  JointTemperatureRegister;

                    /// <summary> The measured joint position. </summary>
                    array<double>^  MeasuredJointPositionRegister;

					/// <summary> The estimated joint velocity. </summary>
					array<double>^  MeasuredJointVelocityRegister;

                    /// <summary> Commanded joint position reported by the LBR controller. </summary>
                    array<double>^  ReportedCommandedJointPositionRegister;

                    /// <summary> Commanded joint offset reported by the LBR controller.  </summary>
                    array<double>^  ReportedCommandedJointOffsetRegister;

                    /// <summary> Measured Cartesian position.  </summary>
                    array<double>^  MeasuredCartesianPositionRegister;

                    /// <summary> Commanded cartesian position reported by the LBR controller. </summary>
                    array<double>^  ReportedCommandedCartesianPositionRegister;

                    /// <summary> Commanded cartesian offset reported by the LBR controller. </summary>
                    array<double>^  ReportedCommandedCartesianOffsetRegister;

                    /// <summary> Measured joint torque. </summary>
                    array<double>^  MeasuredJointTorqueRegister;

                    /// <summary> Estimate of torques that is due to external force. </summary>
                    array<double>^  MeasuredExternalJointTorqueRegister;

                    /// <summary> Estimated force/torque applied to tool control point. </summary>
                    array<double>^  EstimatedTCPForceTorqueRegister;

                    /// <summary> Current numercial manipulator jacobian. </summary>
                    array<double>^  JacobianRegister;

                    /// <summary> Current numerical mass matrix. </summary>
                    array<double>^  MassMatrixRegister;

                    /// <summary> Torque required for gravity comp in current config. </summary>
                    array<double>^  GravityCompensationRegister;

                    /// <summary> Last joint position command sent over the FRI interface. </summary>
                    array<double>^  LastIssuedJointPositionCommandRegister;

                    /// <summary> Initializes a new instance of the <c>FeedbackMessageManaged</c> class. </summary>
                    FeedbackMessageManaged();

                    /// <summary> Initializes a new instance of the <c>FeedbackMessageManaged</c> class from an unmanaged version. </summary>
                    FeedbackMessageManaged(KukaFeedbackMessage *pMessage);

                    /// <summary> Copy from an unmanaged feedbackMessage </summary>
                    void CopyFrom(KukaFeedbackMessage *pMessage);
                private:
                    /// <summary> Allocate the dynamic arrays </summary>
                    void Initialize();

                    /// <summary>Copy from unmanaged array to managed array. Assumed to be same length </summary>
                    template<class T>
                    bool CopyFromUnmanagedArray(T* from, size_t length, array<T>^ to);
                };
            }
        }
    }
}

